#include "Foc.h"
#include "bsp_AS5600.h"


FOC_TypeDef FOC_1={0};
FOC_TypeDef FOC_2={0};

void PWM_init()
{
  HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
}

void FOC_Init(FOC_TypeDef* motor,I2C_HandleTypeDef* i2c,float Umax,uint8_t pole,float rate)
{
	motor->i2c=i2c;
	motor->Umax=Umax;
	motor->pole=pole;
	AS5600_Read(motor,i2c);
    motor->last_motor_angle = motor->motor_angle;
	motor->current_angle=pole * motor->motor_angle;
	motor->rate=rate;
	pid_controller_create(&(motor->FOC_S_PID), 0.0007, 0.5, 0, rate, 6);
	pid_controller_create(&(motor->FOC_P_PID), 0.18, 0, 0, rate, 6);
}

void FOC_Out(FOC_TypeDef* motor,float Uq,float Ud,float motor_angle)
{
	motor->Ud=Ud;
	motor->Uq=Uq;
	
	motor->current_angle=motor->pole * motor_angle;
	float angle=motor->current_angle/57.3248;
	motor->U_alpha=motor->Ud*cosf(angle)-motor->Uq*sinf(angle);
	motor->U_beta=motor->Uq*cosf(angle)+motor->Ud*sinf(angle);
	
	motor->Ua=motor->U_alpha+motor->Umax/2;
	motor->Ub=(sqrtf(3)*motor->U_beta-motor->U_alpha)/2+motor->Umax/2;
	motor->Uc=(-motor->U_alpha-sqrtf(3)*motor->U_beta)/2+motor->Umax/2;
	
    if(motor->Ua>=motor->Umax)
	{
		motor->Ua=motor->Umax;
	}
    if(motor->Ua<=motor->Umax)
	{
		motor->Ua=-motor->Umax;
	}
	
	if(motor->Ub>=motor->Umax)
	{
		motor->Ub=motor->Umax;
	}
	if(motor->Ub<=motor->Umax)
	{
		motor->Ub=-motor->Umax;
	}

	if(motor->Uc>=motor->Umax)
	{
		motor->Uc=motor->Umax;
	}
    if(motor->Uc<=motor->Umax)
	{
		motor->Uc=-motor->Umax;
	}
}

void FOC_Position_Pid(FOC_TypeDef * motor, float target_angle)
{
	AS5600_Read(motor,motor->i2c);
	float error = target_angle-motor->motor_angle;
	if (error >= 180)
	{
		error -= 360;
	}
	else if (error <= -180)
	{
		error += 360;
	}
	pid_error_input(&(motor->FOC_P_PID), error);
	motor->Uq = -pid_output(&(motor->FOC_P_PID));
}

void FOC_Speed_Pid(FOC_TypeDef * motor, float target_speed)
{
	AS5600_Read(motor,motor->i2c);
    float error = motor->motor_angle - motor->last_motor_angle;

    if (error > 180.0f) 
    {
        error -= 360.0f;
    }else if (error < -180.0f) 
    {
        error += 360.0f;
    }

    motor->Speed = error/0.001f;
	float speed_error = target_speed - motor->Speed;
    motor->last_motor_angle = motor->motor_angle;
	pid_error_input(&(motor->FOC_S_PID), speed_error);
	motor->Uq = -pid_output(&(motor->FOC_S_PID));
}

void FOC_Position_exe(FOC_TypeDef* motor,float target_angle)
{
	FOC_Position_Pid(motor, target_angle);
	FOC_Out(motor,motor->Uq,motor->Ud,motor->motor_angle);
	__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,motor->Ua*2000/motor->Umax);
    __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_2,motor->Ub*2000/motor->Umax);
    __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_3,motor->Uc*2000/motor->Umax);
}

void FOC_Speed_exe(FOC_TypeDef* motor,float target_speed)
{
	FOC_Speed_Pid(motor, target_speed);
	FOC_Out(motor,motor->Uq,motor->Ud,motor->motor_angle);
	__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,motor->Ua*2000/motor->Umax);
    __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_2,motor->Ub*2000/motor->Umax);
    __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_3,motor->Uc*2000/motor->Umax);
}

void FOC_Sin_Ctrl(FOC_TypeDef* motor)
{
	motor->start_angle++;
	__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,250+250*sinf(motor->start_angle/57.3));
	__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_2,250+250*sinf(motor->start_angle/57.3+2*PI/3));
	__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_3,250+250*sinf(motor->start_angle/57.3+4*PI/3));
	HAL_Delay(ctrl_time);
}
